
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs_mavlink.c
  * @author     baiyang
  * @date       2022-11-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gcs_mavlink.h"

#include "gcs.h"

#ifdef MAVLINK_SEPARATE_HELPERS
// Shut up warnings about missing declarations; TODO: should be fixed on
// mavlink/pymavlink project for when MAVLINK_SEPARATE_HELPERS is defined
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmissing-declarations"
#include <c_library_v2/mavlink_helpers.h>
#pragma GCC diagnostic pop
#endif

#include <rtthread.h>

#include <common/console/console.h>
#include <serial_manager/gp_serial.h>
#include <serial_manager/gp_serial_device.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
rt_device_t mavlink_comm_port[MAVLINK_COMM_NUM_BUFFERS];
bool gcs_alternative_active[MAVLINK_COMM_NUM_BUFFERS];

// per-channel lock
static struct rt_mutex chan_locks[MAVLINK_COMM_NUM_BUFFERS];
static bool chan_discard[MAVLINK_COMM_NUM_BUFFERS];

mavlink_system_t mavlink_system = {7,1};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Check for available transmit space on the nominated MAVLink channel
///
/// @param chan     Channel to check
/// @returns        Number of bytes available
uint16_t mavcomm_get_txspace(mavlink_channel_t chan)
{
    if (!mavcomm_valid_channel(chan) || mavlink_comm_port[chan] == NULL) {
        return 0;
    }

    gcs_mavlink_t link = gcs_chan(chan);
    if (link == NULL) {
        return 0;
    }

    return gcsmav_txspace(link);
}

/*
  send a buffer out a MAVLink channel
 */
void mavcomm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint16_t len)
{
    if (!mavcomm_valid_channel(chan) || mavlink_comm_port[chan] == NULL || chan_discard[chan]) {
        return;
    }
#if HAL_HIGH_LATENCY2_ENABLED
    // if it's a disabled high latency channel, don't send
    gcs_mavlink_t link = gcs_chan(chan);
    if (!gcsmav_should_send(link)) {
        return;
    }
#endif
    if (gcs_alternative_active[chan]) {
        // an alternative protocol is active
        return;
    }
    const size_t written = SerialManager_write(mavlink_comm_port[chan], buf, len);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    if (written < len) {
        console_panic("Short write on UART: %lu < %u", (unsigned long)written, len);
    }
#else
    (void)written;
#endif
}

/*
  lock a channel for send
  if there is insufficient space to send size bytes then all bytes
  written to the channel by the mavlink library will be discarded
  while the lock is held.
 */
void mavcomm_send_lock(mavlink_channel_t chan_m, uint16_t size)
{
    const uint8_t chan = (uint8_t)chan_m;

    rt_mutex_take(&chan_locks[chan], RT_WAITING_FOREVER);
    if (SerialManager_tx_space(mavlink_comm_port[chan]) < size) {
        chan_discard[chan] = true;
        gcs_out_of_space_to_send(chan_m);
    }
}

/*
  unlock a channel
 */
void mavcomm_send_unlock(mavlink_channel_t chan_m)
{
    const uint8_t chan = (uint8_t)chan_m;
    chan_discard[chan] = false;
    rt_mutex_release(&chan_locks[chan]);
}

/*
  return reference to GCS channel lock, allowing for
  HAVE_PAYLOAD_SPACE() to be run with a locked channel
 */
rt_mutex_t mavcomm_chan_lock(mavlink_channel_t chan)
{
    return &chan_locks[(uint8_t)chan];
}

void mavcomm_init_chan_locks(uint8_t instance)
{
    char name[RT_NAME_MAX + 1];

    if (instance < MAVLINK_COMM_NUM_BUFFERS) {
        rt_snprintf(name, RT_NAME_MAX, "ch_lock%d", instance);
        rt_mutex_init(&chan_locks[instance], name, RT_IPC_FLAG_PRIO);
    }
}
/*------------------------------------test------------------------------------*/


